惯性导航解算就是从上一时刻的导航信息推导本时刻的导航信息，包括姿态、速度、位置。具体来讲，就是构建当前时刻导航信息与上一时刻导航信息、

运动输入 (角速度、加速度) 之间的关系，确切地说，是微分关系。姿态、速度、位置的更新中，尤以姿态更新的方法最多，计算也最为复杂，

因为姿态有不同的表示形式，而且具有非线性和不可交换性，因此它是整个导航解算的重中之重。

在低精度惯性导航中，以当地平面坐标系作为导航系，不考虑地球模型和地球自转角速度，这种做法当然也仅适用于低精度导航，

对于本篇文章来讲，重点在于讲解导航解算的思路，简单的模型更利于把问题讲清楚，因此对于高精度模型并不做详细推导。

姿态有三种表示形式：欧拉角、旋转矩阵、四元数，每种表示形式对应一种更新方法，由于欧拉角存在万向锁问题，不能在所有导航问题中具有通用性，

因此本文只介绍基于 旋转矩阵和四元数 的姿态更新。另外，并不是所有IMU器件都以角速度形式输出旋转信息，有的会输出角增量，

即角速度在一个采样周期内的积分。信息信息不同也会导致方法上的差异，要区别对待。

方向余弦矩阵更新法的基础是姿态矩阵微分方程：

其中 表示向量 构成的反对称矩阵

这个微分方程的解就是我们要的姿态更新公式，多数教科书上给出的关于这个微分方程的解法是毕卡(Picard)迭代法，它是求解微分方程的一种常见形式，

该方法的主要思路是从初始值开始逐次带入。首先对(1)式积分，得到

按照矩阵指数形式转三角函数形式的方法，可以得到

把上式的推导过程，应用到k时刻到k+1时刻的姿态更新的需求中，则有

四元数更新法的基础就是四元数微分方程：

其中 o 表示四元数乘法。其实四元数更新的套路和上面旋转矩阵更新法是一样的，也是使用毕卡迭代法求解微分方程，这里不再展示详细过程了，直接给出结果。

其中 是角增量对应的单位矢量。

同样地，四元数更新方法也直接给出结果

上面介绍的两种方法都是围绕角增量进行的，等效旋转矢量也是在角增量的基础上做补偿，实际上，角增量也是通往姿态更新的必经途径，

基于角速度的更新也是先用角速度计算出角增量，再使用角增量方法进行更新，所以本小节的主要内容就是介绍怎样从角速度得到角增量。

“角增量是角速度的积分”这句话并不能直接带给我们答案，因为离散时间模型里不存在真正意义上的积分，因此有了以下这些方法。

这是一种非常朴素的思想，假设角速度是固定值，那么角增量就是角速度和时间的乘积了。即

欧拉法的假设过于理想，因此误差比较大，比这个稍微精确的模型是假设角速度线性变化，那么有

即，前后时刻角速度的中值认为是整个区间的等效角速度，这也是中值积分的含义。

如果认为一阶模型精度不够，那么就使用高阶模型，四阶龙格库塔就是惯性导航中常用的一种逼近方法。它的原理可以用一个例子解释。对于如下方程

当然也可以设计更高阶的拟合，但是它的精度并不一定会提高很多，反而会增加计算量，所以高精度时使用四阶龙格库塔也就够了。

速度更新是基于速度关于加速度的微分方程

其中a是导航系下，减掉重力加速度以后的载体加速度，可以很容易得到

由于大多数加速度计输出的不是速度增量，而是加速度，因此在离散系统里，同样需要使用拟合方法计算速度，

上面所说的欧拉法、中值法、四阶龙格库塔法都可以使用，工程中最常使用的是中值法。

位置关于速度的微分方程为

可以得到

套路一样，使用中值法解决这个计算。

多数教材中，在介绍基于多字样补偿等效旋转矢量法的姿态解算时使用的都是四元数更新，容易造成一种误解，认为等效旋转矢量法就得使用四元数才行，

其实通过上面的介绍可以发现，基于角增量的解算方法和基于等效旋转矢量的解算方法具有同样的形式，只是后者对角增量做了不可交换误差补偿，

这个补偿的计算和后面的解算是独立的，因此基于旋转矩阵的更新方法同样可以使用。之所以多数教材中使用四元数，是因为早期惯导系统的计算机板计算能力有限，

四元数计算简单，节省计算量而已。

整个更新过程中，需要选择阶次的环节有这样几个:

1)计算不可交换误差补偿量时，需要设置角速度的阶次，这对应着子样数。

2)由角速度计算角增量时需要选择逼近方法的阶次。

3)速度更新时需要选择加速度变化的阶次

4)位置更新时需要选择速度变化的阶次

每一次都可以选择很高阶的假设，但是所有高阶假设都要求函数足够光滑，而实际上所有的输入都是有噪声的，因此当阶次达到一定程度，

再去提高也并不能提高计算精度，因此需要根据情况选择合适的阶次，而不是越高越好。

 正在上传…重新上传取消